// model
#include "model.h"
#include <stdio.h>
#include <stdlib.h>
#include "glotech_headers.h"
#include <math.h>

#ifndef ERROR_MARGIN
	#define ERROR_MARGIN 0.122173
#endif

struct node{
	float time;
	float pos_x;
	float pos_y;
	float pos_z;
	float theta_x;
	float theta_y;
	float theta_z;
	float theta_marge_min_x;
	float theta_marge_min_y;
	float theta_marge_min_z;
	float theta_marge_max_x;
	float theta_marge_max_y;
	float theta_marge_max_z;
	float radius;
	struct node * next;
};

typedef struct node NODE;

struct queue{
	int cnt;
	float time_delta;
	NODE * head;
	NODE * tail;
};

typedef struct queue QUEUE;

typedef struct view VIEW;

struct view{
	int centrex;
	int centrey;
	float multiplier;
	Tobjet P;	
	Tobjet PProbeInit;
	Tobjet PProbe;
};


int difference(NODE *n, NODE * n2){
		double dist, rot;                         //if(((theoretical-measured)/theoretical) * 100 > ERROR_MARGIN)
		float tx = n->pos_x - n2->pos_x;
		float ty = n->pos_y - n2->pos_y;
		float tz = n->pos_z - n2->pos_z;

		float rx = n->theta_x - n2->theta_x;
		float ry = n->theta_y - n2->theta_y;
		float rz = n->theta_z - n2->theta_z;
		
		

		dist = sqrt(double(pow(tx,2))+(pow(ty,2))+(pow(tz,2)));

		rot =  sqrt(double(pow(rx,2))+(pow(ry,2))+(pow(rz,2)));


		if ((dist < n->radius) && (rot < ERROR_ANGLE))
		
			return SUCCESS;
		else
			return FAILURE;
}





void calculate_radius_node(NODE * n){
    

	n->radius = 5; //sqrt ((double (ERROR_POSITION * ERROR_POSITION))/(2.0 * PI));


	
	   n->theta_marge_max_x = n->theta_x + ERROR_ANGLE;
	   n->theta_marge_min_x = n->theta_x - ERROR_ANGLE;
	
       n->theta_marge_max_y = n->theta_y + ERROR_ANGLE;
	   n->theta_marge_min_y = n->theta_y - ERROR_ANGLE;
	
       n->theta_marge_max_z = n->theta_z + ERROR_ANGLE;
	   n->theta_marge_min_z = n->theta_z - ERROR_ANGLE;
	
}





void calculate_radius(QUEUE * q){
	NODE * next =  q->head->next;

	while(next != 0){
		calculate_radius_node(next);
		next = next->next;
	}
	calculate_radius_node(q->tail);
}

MODEL * init_model(void){
	MODEL * m;
	QUEUE * q_expert;
	QUEUE * q_tech;

	m = (MODEL *) malloc(sizeof(MODEL));
	if(m == NULL)
		return NULL;
	q_expert = init_queue();
	q_tech = init_queue();
	if((q_expert == NULL) || (q_tech == NULL)){
		free(m);
		free(q_tech);
		free(q_expert);
		return NULL;
	}
	m->q_expert = q_expert;
	m->q_technician = q_tech;
	return m;
}

void destroy_model(MODEL * m){
	destroy_queue(m->q_expert);
	destroy_queue(m->q_technician);
	free(m);
}

QUEUE * init_queue(void){
	QUEUE * new_queue;
	new_queue = (QUEUE *) malloc(sizeof(QUEUE));
	if(new_queue == NULL)
		return NULL;
	new_queue->cnt = 0;
	new_queue->head = 0;
	new_queue->tail = 0;
	new_queue->time_delta = 0;
	return new_queue;
}

int push(QUEUE * q, float pos_x, float pos_y, float pos_z, float theta_x, float theta_y, float theta_z, float time){
	NODE * new_node;
	new_node = (NODE *) malloc(sizeof(NODE));
	if(new_node == NULL){
		return FAILURE;
	}
	new_node->time = time;
	new_node->next = 0;
	new_node->pos_x = pos_x;
	new_node->pos_y = pos_y;
	new_node->pos_z = pos_z;
	new_node->theta_x = theta_x;
	new_node->theta_y = theta_y;
	new_node->theta_z = theta_z;
	new_node->theta_marge_min_x = 0;
	new_node->theta_marge_min_y = 0;
	new_node->theta_marge_min_z = 0;
	new_node->theta_marge_max_x = 0;
	new_node->theta_marge_max_y = 0;
	new_node->theta_marge_max_z = 0;
	new_node->radius = 0;
	

	if(q->cnt == 0){
		q->head = new_node;
		q->tail = new_node;
	} else {
		q->tail->next = new_node;
		q->tail = new_node;
	}
	q->cnt++;
	return SUCCESS;
}

NODE * pop(QUEUE * q){
	NODE * poped;
	if(q->cnt == 0)
		return NULL;
	poped = q->head;
	q->head = q->head->next;
	q->cnt--;
	return poped;
}

int is_queue_empty(QUEUE * q){
	if(q->cnt == 0)
		return YES;
	return NO;
}

void free_node(NODE * n){
	free(n);
}

void destroy_queue(QUEUE * q){
	NODE * n;
	while(q->cnt != 0){
		n = pop(q);
		free_node(n);
	}
	free(q);
}

void print_node(NODE * n){
	printf("Node: 0x%x\n", n);
	printf("Time: %f\n",n->time);
	printf("Pos X: %f\tTheta X: %f\n", n->pos_x, n->theta_x);
	printf("Pos Y: %f\tTheta Y: %f\n", n->pos_y, n->theta_y);
	printf("Pos Z: %f\tTheta Z: %f\n\n", n->pos_z, n->theta_z);
}

void print_queue(QUEUE * q){
	NODE * next =  q->head->next;
	int cnt = 0;

	printf("Queue: 0x%x\n", q);
	printf("Node Count: %i\n", q->cnt);
	printf("Head: 0x%x\n", q->head);
	printf("Tail: 0x%x\n\n", q->tail);
	
	print_node(q->head);
	while(next != 0){
		print_node(next);
		next = next->next;
		cnt++;
	}
}

QUEUE * csv_queue;

char * getline(char * line ){
	return 0;
}

int init_driver_csv(const char * filename){
	FILE * fp;
	QUEUE * q;
	//ssize_t read;
	char read[2048];
	char buffer[2048];
	float data_buffer[64];
	int i=0,j=0,k=0;
	int t;
	
	q = init_queue();
	if(q == NULL){
		return FAILURE;
	}

	fp = fopen(filename , "r");
	if(fp == NULL)
		return FAILURE;
	while((fgets(read, 256, fp)) != NULL){
		for(i=0;i<sizeof(read);i++){
			if(read[i] == ','){
				buffer[k] = '\0';
				data_buffer[j] = atof(buffer);
				k=0;
				j++;
				// Ja... Leute werden mich hassen, fur diesen einen xD
			} else if((read[i] == '\n') || (read[i] == '\r')){
				data_buffer[j] = atof(buffer);
				j=0;
				if(push(q, data_buffer[3], data_buffer[2],data_buffer[1], data_buffer[4], data_buffer[5], data_buffer[6], data_buffer[0]) == FAILURE)
					return FAILURE;
				break;
			} else if(((read[i] >= 65) && (read[i] <= 90)) || ((read[i] >= 97) && (read[i] <= 122))){
				break;
			} else {
				buffer[k] = read[i];
				k++;
			}
		}
	}

	csv_queue = q;

	fclose(fp);
	return SUCCESS;
}

QUEUE * load_csv(const char * filename){
	FILE * fp;
	QUEUE * q;
	//ssize_t read;
	char read[4096];
	char buffer[4096];
	float data_buffer[128];
	int i=0,j=0,k=0;
	int t;
	
	q = init_queue();
	if(q == NULL){
		return NULL;
	}

	fp = fopen(filename , "r");
	if(fp == NULL)
		return NULL;
	while((fgets(read, 256, fp)) != NULL){
		for(i=0;i<sizeof(read);i++){
			if(read[i] == ','){
				buffer[k] = '\0';
				data_buffer[j] = atof(buffer);
				k=0;
				j++;
				// Ja... Leute werden mich hassen, fur diesen einen xD
			} else if((read[i] == '\n')){
				buffer[k] = '\0';
				data_buffer[j] = atof(buffer);
				j=0;
				k=0;
				if(push(q, data_buffer[4], data_buffer[5],data_buffer[6], data_buffer[3] * (3.14159265/180), data_buffer[2]* (3.14159265/180), data_buffer[1] * (3.14159265/180), data_buffer[0]) == FAILURE)
					return NULL;
				break;
			} else if(((read[i] >= 65) && (read[i] <= 90)) || ((read[i] >= 97) && (read[i] <= 122))){
				break;
			} else {
				buffer[k] = read[i];
				k++;
			}
		}
	}

	fclose(fp);
	q->time_delta = q->head->next->time;
	return q;
}


NODE * model_get_data(void){
	return pop(csv_queue);
}


float get_pos_x(NODE * n){
	return n->pos_x;
}
float get_pos_y(NODE * n){
	return n->pos_y;
}
float get_pos_z(NODE * n){
	return n->pos_z;
}
float get_theta_x(NODE * n){
	return n->theta_x;
}
float get_theta_y(NODE * n){
	return n->theta_y;
}
float get_theta_z(NODE * n){
	return n->theta_z;
}
float get_time(NODE * n){
	return n->time;
}
